//
//  Rover.m
//  SSE
//
//  Created by Matt Roman on 10/11/07.
//  Copyright 2007 __MyCompanyName__. All rights reserved.
//

#import "Rover.h"

static GLfloat norms[60] =	{1,0,0,  1,0,0, -1,0,0, -1,0,0,
							1,0,0,  1,0,0, -1,0,0, -1,0,0,
							0,1,0,  0,1,0,  0,1,0,  0,1,0,
							0,-1,0, 0,-1,0, 0,-1,0, 0,-1,0,
							0,0,1,  0,0,1,  0,0,1,  0,0,1};

// BODY ///////////////////////////////////////////////////////////////////////
//		 Z
//       |
//		 TOP       
//    v6----- v5 
//   /|      /|
//  v7------v4|
//  | |     | |
//  | |v2---|-|v1
//  |/   .  |/ -------> X FRONT
//  v3------v0
//    LEFT

@implementation Rover

- (id)initRover
{
	[super init];
	mode = AUTO;
	[self setRoverPositionX:0 Y:0 Z:0];
	[self setAlpha:0];
	[self setSpeed:0];
	
	body = [[Crate alloc]initCrateAlpha:alpha
									  X:0
									  Y:0
									  Z:0
									  L:LENGTH
									  W:WIDTH
									  H:HEIGHT];
	[body setColorR:1 G:1 B:1 A:1];
	
	
	mark[0] = LENGTH/2.0;		// TRIANGLE
	mark[1] = 0.0;
	mark[2] = HEIGHT+0.01;
	
	mark[3] = 0.0;
	mark[4] = WIDTH/2.0;
	mark[5] = HEIGHT+0.01;
	
	mark[6] = 0.0;
	mark[7] = -WIDTH/2.0;
	mark[8] = HEIGHT+0.01;
	 
	return self;
}

- (void)setRoverPositionX:(float)x Y:(float)y Z:(float)z
{
	position.x = x;
	position.y = y;
	position.z = z;
}
- (void)setRoverPosition:(Point3f *)pos
{
	position = *pos;
}

- (Point3f *)getRoverPosition
{
	return &position;
}

- (void)setAlpha:(float)a
{
	alpha = a;
}

- (float *)getRoverAlpha
{
	return &alpha;
}

- (void)Alpha:(float)a
{
	alpha += a;
}

- (Vector3f *)getHeading
{
	heading.a = cos(alpha);
	heading.b = sin(alpha);
	heading.c = 0.0;
	return &heading;
}

- (Vector3f *)getGoalVector
{
	return &ToGoal;
}

- (void)setSpeed:(float)spd
{
	speed = spd;
}

- (void)Speed:(float)spd
{
	speed += spd;
}

- (int *)getRoverSpeed
{
	return &speed;
}

- (void)setStart:(Point3f *)s
{
	start = *s;
}

- (Point3f *)getStart
{
	return &start;
}

- (void)setGoal:(Point3f *)g
{
	goal = *g;
}

- (Point3f *)getGoal
{
	return &goal;
}

- (void)setWayX:(float)x Y:(float)y
{
	way.x = x;
	way.y = y;
	way.z = 0.0;
}

///////////////////////////////////////////////////////////////////////////////////////
// Draws the rover
- (void)drawRover
{
	int i;
	/*
	
	glEnableClientState(GL_NORMAL_ARRAY);			// ENABLE THE USE OF OBJECT ARRAYS
	glEnableClientState(GL_VERTEX_ARRAY);
	//glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_DST_COLOR);
	glNormalPointer(GL_FLOAT, 0, norms);
	glVertexPointer(3, GL_FLOAT, 0, body);
	*/
	glPushMatrix();
	glTranslatef(position.x,position.y,position.z);
	glRotatef(RADTODEG(alpha), 0.0, 0.0, 1.0);
	/*glBegin(GL_LINE_STRIP);
		glColor4f(CYAN);
		glVertex3f(SENSEDIST*cos(SENSEANGLE),SENSEDIST*sin(SENSEANGLE),0);
		glVertex3f(0,0,0);
		glVertex3f(SENSEDIST*cos(SENSEANGLE),-SENSEDIST*sin(SENSEANGLE),0);
	glEnd();
	 */
		[body drawCrate];
	glBegin(GL_TRIANGLES);
		glColor4f(BLUE);
		glNormal3f(0.0,0.0,1.0);
		glVertex3fv(mark);
		glVertex3fv(mark+3);
		glVertex3fv(mark+6);
	glEnd();

	if(senseRadius != 0){
		glColor4f(ORANGE);
		glBegin(GL_LINE_LOOP);
		for(i=0; i<360; i++){
			float degInRad = DEGTORAD(i);
			glVertex3f(cos(degInRad)*senseRadius,sin(degInRad)*senseRadius,0.05);
		}
		glEnd();
	}
	glPopMatrix();
	
	//glDisableClientState(GL_NORMAL_ARRAY);			// DISABLE THE USE OF OBJECT ARRAYS
	//glDisableClientState(GL_VERTEX_ARRAY);
	
}

////////////////////////////////////////////////////////////////////////////////////////
// updates the rovers goal vector and magnitude 
// updates alpha for heading information
// updates rover speed and xy position
-(void)updateRover
{
	float del_p;
	
	DisttoGoal = PntstoVector(&position,&goal,&ToGoal);
	//DisttoWay = PntstoVector(&position,&way,&ToWay);
	
	if(alpha >= TWOPI) alpha -= TWOPI;
	else if(alpha < 0.0) alpha += TWOPI;
	
	if(speed >= MAXSPEED) speed = MAXSPEED;
	else if(speed <= -MAXSPEED) speed = -MAXSPEED;
	
	del_p = speed * LOOPTIME/100.0;
	
	position.x = position.x + del_p * [self getHeading]->a;
	position.y = position.y + del_p * [self getHeading]->b;
}

- (void) setSenseRadius:(float)sens
{
	senseRadius = sens;
}

- (float *)senseRadius
{
	return &senseRadius;
}

- (void) reset
{
	mode = AUTO;
	[self setRoverPositionX:0 Y:0 Z:0];
	[self setAlpha:0];
	[self setSpeed:0];
}
@end
